﻿using System;
using System.Drawing;
using System.Collections.Generic;
//using Microsoft.DirectX;
using System.IO;

namespace System
{
	public partial struct V3D
	{
		public double X, Y, Z;
		public bool Valid;
		//Constructors
		public V3D(double x, double y, double z)
		{
			X = x;
			Y = y;
			Z = z;
			Valid = !double.IsNaN(x) && !double.IsNaN(y) && !double.IsNaN(z);
		}
		public V3D(double x, double y)
			: this(x, y, 0)
		{
		}
		public V3D(double[] V)
			: this()
		{
			if (V.Length > 2)
			{
				X = V[0];
				Y = V[1];
				Z = V[2];
				Valid = true;
			}
			else
			{
				X = Y = Z = double.NaN;
				Valid = false;
			}
		}
		public V3D(String S)
		{
			String[] C = S.Split(';');
			try
			{
				X = double.Parse(C[0]);
				Y = double.Parse(C[1]);
				Z = double.Parse(C[2]);
				Valid = true;
			}
			catch
			{
				X = Y = Z = double.NaN;
				Valid = false;
			}
		}
		public static void Swap(ref V3D A, ref V3D B)
		{
			V3D Temp = A;
			A = B;
			B = Temp;
		}
		public static V3D Min(V3D A, V3D B)
		{
			return new V3D(Math.Min(A.X, B.X), Math.Min(A.Y, B.Y), Math.Min(A.Z, B.Z));
		}
		public static V3D Max(V3D A, V3D B)
		{
			return new V3D(Math.Max(A.X, B.X), Math.Max(A.Y, B.Y), Math.Max(A.Z, B.Z));
		}
		public static int CompareXYZ(V3D P1, V3D P2)
		{
			if (P1.X < P2.X)
				return -1;
			else if (P1.X > P2.X)
				return 1;
			else if (P1.Y < P2.Y)
				return -1;
			else if (P1.Y > P2.Y)
				return 1;
			else if (P1.Z < P2.Z)
				return -1;
			else if (P1.Z > P2.Z)
				return 1;
			else
				return 0;
		}
		public static bool operator <(V3D P1, V3D P2)
		{
			return CompareXYZ(P1, P2) == -1;
		}
		public static bool operator >(V3D P1, V3D P2)
		{
			return CompareXYZ(P1, P2) == 1;
		}
		public static bool operator <=(V3D P1, V3D P2)
		{
			return CompareXYZ(P1, P2) < 1;
		}
		public static bool operator >=(V3D P1, V3D P2)
		{
			return CompareXYZ(P1, P2) > -1;
		}
		public override int GetHashCode()
		{
			return ((IntPtr)X).ToInt32();
		}
		public override bool Equals(object obj)
		{
			if (obj is V3D)
				return (V3D)obj == this;
			else
				return false;
		}
		public static bool operator ==(V3D v1, V3D v2)
		{
			double precision = 1e-12;
			if (object.ReferenceEquals(v1, v2))
				return true;
			else if ((object)v1 == null || (object)v2 == null)
				return false;
			else
				return
					((v1.X == 0 && v2.X == 0) || (Math.Abs(v1.X - v2.X) / Math.Max(Math.Abs(v1.X), Math.Abs(v2.X)) < precision))
					&& ((v1.Y == 0 && v2.Y == 0) || (Math.Abs(v1.Y - v2.Y) / Math.Max(Math.Abs(v1.Y), Math.Abs(v2.Y)) < precision))
					&& ((v1.Z == 0 && v2.Z == 0) || (Math.Abs(v1.Z - v2.Z) / Math.Max(Math.Abs(v1.Z), Math.Abs(v2.Z)) < precision));
		}
		public static bool operator !=(V3D v1, V3D v2)
		{
			return !(v1 == v2);
		}
		public static V3D operator +(V3D v1, V3D v2)
		{
			if (v1.Valid && v2.Valid)
				return new V3D(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
			else
				return V3D.Null;
		}
		public static V3D operator -(V3D v1, V3D v2)
		{
			if (v1.Valid && v2.Valid)
				return new V3D(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
			else
				return V3D.Null;
		}
		public static V3D operator -(V3D v1)
		{
			if (v1.Valid)
				return new V3D(-v1.X, -v1.Y, -v1.Z);
			else
				return V3D.Null;
		}
		public static V3D operator /(V3D v1, double n)
		{
			if (v1.Valid)
				return new V3D(v1.X / n, v1.Y / n, v1.Z / n);
			else
				return V3D.Null;
		}
		public static double operator *(V3D v1, V3D v2)
		{
			if (v1.Valid && v2.Valid)
				return (v1.X * v2.X) + (v1.Y * v2.Y) + (v1.Z * v2.Z);
			else
				return double.NaN;
		}
		public static V3D operator *(V3D v1, double n)
		{
			if (v1.Valid)
				return new V3D(v1.X * n, v1.Y * n, v1.Z * n);
			else
				return V3D.Null;
		}
		public static V3D operator *(double n, V3D v1)
		{
			return v1 * n;
		}
		public static V3D operator %(V3D v1, V3D v2)
		{
			if (v1.Valid && v2.Valid)
				return new V3D(v1.Y * v2.Z - v2.Y * v1.Z, v1.Z * v2.X - v2.Z * v1.X, v1.X * v2.Y - v2.X * v1.Y);
			else
				return V3D.Null;
		}
		//Conversion
		public override String ToString()
		{
			if (Valid)
				return X.ToString() + ";" + Y.ToString() + ";" + Z.ToString();
			else
				return "<<Invalid>>";
		}
		public String ToString(int i, int j)
		{
			int a;
			String format, result;
			format = "{0:";
			for (a = 0; a < i; a++)
				format += "0";
			if (j > 0)
			{
				format += ".";
				for (a = 0; a < j; a++)
					format += "0";
			}
			format += "}";
			result = '(' + String.Format(format, X);
			result += ';' + String.Format(format, Y);
			result += ';' + String.Format(format, Z) + ')';
			return result;
		}
		public double[] ToArray()
		{
			return new double[] { X, Y, Z };
		}
		public double Length
		{
			get
			{
				return Math.Sqrt(LengthSq);
			}
		}
		public double LengthSq
		{
			get
			{
				return X * X + Y * Y + Z * Z;
			}
		}
		public V3D Unit
		{
			get
			{
				return this * (1.0d / this.Length);
			}
			set
			{
				this = value;
				double d = this.Length;
				this.X = this.X / d;
				this.Y = this.Y / d;
				this.Z = this.Z / d;
			}
		}
		public void DoRot(V3D a, double angle)
		{
			this = this.Rot(a, angle);
			//this.X = res.X;
			//this.Y = res.Y;
			//this.Z = res.Z;
		}
		// Math
		public static double AngleCos(V3D v1, V3D v2)
		{
			return (v1 * v2) / (v1.Length * v2.Length);
		}
		public static double Angle(V3D v1, V3D v2)
		{
			double x = (v1 * v2) / (v1.Length * v2.Length);
			if (x >= 1)
				return 0;
			else if (x <= -1)
				return Math.PI;
			else
				return Math.Acos(x);
		}
		public static double AngleD(V3D v1, V3D v2)
		{
			return Angle(v1, v2) / Math.PI * 180;
		}
		public static double AbsAngle(V3D vref, V3D v, V3D Up)
		{
			V3D Ref2D = vref.CrossPart(Up);
			V3D v2D = v.CrossPart(Up);
			return Angle(Ref2D, v2D) * ((Ref2D % v) * Up > 0 ? 1 : -1);
		}
		public static double AbsAngleD(V3D vref, V3D v, V3D Up)
		{
			return AbsAngle(vref, v, Up) / Math.PI * 180;
		}
		public static double Angle360(V3D vref, V3D v, V3D Up)
		{
			vref = vref.CrossPart(Up);
			v = v.CrossPart(Up);
			double d = (vref * v) / (vref.Length * v.Length);
			if (d >= 1)
				return 0;
			else if (d <= -1)
				return Math.PI;
			else
				return (vref % v) * Up < 0 ? Angle(vref, v) : Math.PI * 2 - Angle(vref, v);
		}
		public static double Angle360D(V3D vref, V3D v, V3D Up)
		{
			return Angle360(vref, v, Up) / Math.PI * 180;
		}

		public V3D ParallelPart(V3D To)
		{
			return To.Unit * Length * AngleCos(this, To);
		}
		public V3D Tangential(V3D To)
		{
			return Unit * To.Length / AngleCos(this, To);
		}
		public V3D CrossPart(V3D To)
		{
			return this - ParallelPart(To);
		}
		public V3D Interpolate(V3D v1, double n)
		{
			return this * (1 - n) + v1 * n;
		}
		public static V3D Interpolate(V3D v1, V3D v2, double n)
		{
			return v1 * (1 - n) + v2 * n;
		}

		//Distances
		public Line3D DistanceLine(Plane3D P)
		{
			return new Line3D(this, P.Origin + (this - P.Origin).CrossPart(P.Normal));
		}
		public double Distance(Plane3D P)
		{
			Line3D D = DistanceLine(P);
			if (D.Valid)
				return D.Length;
			else
				return double.NaN;
		}
		/*
		public Line3D DistanceLine(Triangle3D T)
		{
			Line3D Res = DistanceLine((Plane3D)T);
			if (Res.Valid && !T.PointIn(Res.B))
			{
				if ((T.a.Dir % T.Normal) * (Res.B - T.A) > 0)
					Res = DistanceLine(T.a);
				else if ((T.b.Dir % T.Normal) * (Res.B - T.B) > 0)
					Res = DistanceLine(T.b);
				else
					Res = DistanceLine(T.c);
			}
			return Res;
		}
		public double Distance(Triangle3D T)
		{
			Line3D D = DistanceLine(T);
			if (D.Valid)
				return D.Length;
			else
				return -1;
		}
		*/
		//public V2D _ToPlane(V3D P, V3D N){}

		//Misc
		public static V3D Null
		{
			get
			{
				return new V3D(double.NaN, double.NaN, double.NaN);
			}
		}
		public static V3D Black
		{
			get
			{
				return new V3D(0, 0, 0);
			}
		}
		public static V3D Red
		{
			get
			{
				return new V3D(1, 0, 0);
			}
		}
		public static V3D Orange
		{
			get
			{
				return new V3D(1, 0.5, 0);
			}
		}
		public static V3D Yellow
		{
			get
			{
				return new V3D(1, 1, 0);
			}
		}
		public static V3D Green
		{
			get
			{
				return new V3D(0, 1, 0);
			}
		}
		
		public static V3D Cyan
		{
			get
			{
				return new V3D(0, 1, 1);
			}
		}
		public static V3D Blue
		{
			get
			{
				return new V3D(0, 0, 1);
			}
		}
		public static V3D Purple
		{
			get
			{
				return new V3D(1, 0, 1);
			}
		}
		public static V3D White
		{
			get
			{
				return new V3D(1, 1, 1);
			}
		}
		public static V3D VSpect(double n)
		{
			V3D Res = new V3D();
			Res.X = (n < 0.2) ? 1 : (n < 0.4) ? 1 - (n - 0.2) / 0.2 : (n > 0.8) ? (n - 0.8) / 0.2 : 0;
			Res.Y = (n < 0.2) ? n / 0.2 : (n < 0.6) ? 1 : (n < 0.8) ? 1 - (n - 0.6) / 0.2 : 0;
			Res.Z = (n < 0.4) ? 0 : (n < 0.6) ? (n - 0.4) / 0.2 : 1;
			return Res;
		}
		//2D
		public bool In2D(V3D min, V3D max)
		{
			return X >= min.X && X <= max.X && Z >= min.Z && Z <= max.Z;
		}
		public V3D LeftN
		{
			get
			{
				return new V3D(-Y, X, 0);
			}
		}
		public V3D RightN
		{
			get
			{
				return new V3D(Y, -X, 0);
			}
		}
		//Matrix
		/*public Matrix ToColumnMatrix()
		{
			Matrix Res = new Matrix(4, 1);
			Res[0, 0] = X;
			Res[1, 0] = Y;
			Res[2, 0] = Z;
			Res[3, 0] = 1;
			return Res;
		}
		public Matrix ToRowMatrix()
		{
			Matrix Res = new Matrix(1, 4);
			Res[0, 0] = X;
			Res[0, 1] = Y;
			Res[0, 2] = Z;
			Res[0, 3] = 1;
			return Res;
		}
		public Matrix ToTrans()
		{
			Matrix Res = new Matrix(4, 4);
			Res[0, 0] = 1;
			Res[0, 3] = X;
			Res[1, 1] = 1;
			Res[1, 3] = Y;
			Res[2, 2] = 1;
			Res[2, 3] = Z;
			Res[3, 3] = 1;
			return Res;
		}
		public Matrix ToScale()
		{
			Matrix Res = new Matrix(4, 4);
			Res[0, 0] = X;
			Res[1, 1] = Y;
			Res[3, 3] = Z;
			Res[4, 4] = 1;
			return Res;
		}*/
		public double[,] ToRot()
		{
			double[,] Trans = new double[4, 4];
			//Matrix Trans = new Matrix(4, 4);
			double s, c;
			s = Math.Sin(Length / 180.0d * Math.PI);
			c = Math.Cos(Length / 180.0d * Math.PI);
			V3D i = Unit;
			Trans[0, 0] = i.X * i.X + (i.Y * i.Y + i.Z * i.Z) * c;
			Trans[0, 1] = i.X * i.Y * (1 - c) - i.Z * s;
			Trans[0, 2] = i.X * i.Z * (1 - c) + i.Y * s;
			Trans[0, 3] = 0;
			Trans[1, 0] = i.Y * i.X * (1 - c) + i.Z * s;
			Trans[1, 1] = i.Y * i.Y + (i.X * i.X + i.Z * i.Z) * c;
			Trans[1, 2] = i.Y * i.Z * (1 - c) - i.X * s;
			Trans[1, 3] = 0;
			Trans[2, 0] = i.Z * i.X * (1 - c) - i.Y * s;
			Trans[2, 1] = i.Z * i.Y * (1 - c) + i.X * s;
			Trans[2, 2] = i.Z * i.Z + (i.X * i.X + i.Y * i.Y) * c;
			Trans[2, 3] = 0;
			Trans[3, 3] = 1;
			return Trans;
		}
		public V3D Rot(V3D a, double angle)
		{
			if (a.Length * angle != 0)
			{
				double[,] Trans = (a.Unit * angle).ToRot();
				double x = Trans[0, 0] * X + Trans[0, 1] * Y + Trans[0, 2] * Z;
				double y = Trans[1, 0] * X + Trans[1, 1] * Y + Trans[1, 2] * Z;
				double z = Trans[2, 0] * X + Trans[2, 1] * Y + Trans[2, 2] * Z;
				V3D Res = new V3D(x, y, z);
				return Res;
			}
			else
				return this;
		}
		public V3D RotX(double angle)
		{
			double a = angle * Math.PI / 180;
			return new V3D(X, Y * Math.Cos(a) - Z * Math.Sin(a), Y * Math.Sin(a) + Z * Math.Cos(a));
		}
		public V3D RotY(double angle)
		{
			double a = angle * Math.PI / 180;
			return new V3D(X * Math.Cos(a) + Z * Math.Sin(a), Y, -X * Math.Sin(a) + Z * Math.Cos(a));
		}
		public V3D RotZ(double angle)
		{
			double a = angle * Math.PI / 180;
			return new V3D(X * Math.Cos(a) - Y * Math.Sin(a), X * Math.Sin(a) + Y * Math.Cos(a), Z);
		}

		public PointF ToPointF()
		{
			return new PointF((float)X, (float)Y);
		}
		public Point ToPoint()
		{
			return new Point((int)X, (int)Y);
		}
		public Color ToColor()
		{
			return Color.FromArgb((int)(Math.Min(X, 1) * 255), (int)(Math.Min(Y, 1) * 255), (int)(Math.Min(Z, 1) * 255));
		}
		public Color ToColor(byte Alpha)
		{
			return Color.FromArgb(Alpha, (int)(Math.Min(X, 1) * 255), (int)(Math.Min(Y, 1) * 255), (int)(Math.Min(Z, 1) * 255));
		}

		public V3D(byte[] msg, ref int msgptr)
		{
			X = BitConverter.ToDouble(msg, msgptr);
			Y = BitConverter.ToDouble(msg, msgptr + 8);
			Z = BitConverter.ToDouble(msg, msgptr + 16);
			Valid = true;
			msgptr += 24;
		}
		public void Append(List<byte> Res)
		{
			Res.AddRange(BitConverter.GetBytes(X));
			Res.AddRange(BitConverter.GetBytes(Y));
			Res.AddRange(BitConverter.GetBytes(Z));
		}

		public void Save(BinaryWriter W)
		{
			W.Write(X);
			W.Write(Y);
			W.Write(Z);
		}
		public void Load(BinaryReader R)
		{
			X = R.ReadDouble();
			Y = R.ReadDouble();
			Z = R.ReadDouble();
			Valid = true;
		}

		public V3D(BinaryReader R)
		{
			X = R.ReadDouble();
			Y = R.ReadDouble();
			Z = R.ReadDouble();
			Valid = true;
		}
	}
}